#include "encoder.h"
#include "main.h"
#include "spi.h"
#include "usart.h"
#include "arm_math.h"
#include "Motor_Control.h"

ENCODER_DATA Encoder_Data;
ENCODER_DATA Encoder_Data_Real;

float Encoder_Data_Real_Buffer[20];
float Encoder_Data_Test = 0;
float Speed_Data = 0;
float Angle_Data = 0;
float Electrical_Angle = 0;
float Electrical_Angle_Offset = 3.78f;//0.64

uint8_t Encoder_Index = 0;
uint16_t Encoder_Buffer[10];

//发送指令
uint16_t SPI_ReadWrite_OneByte(uint16_t txdata)
{
  //拉低片选引脚
  HAL_GPIO_WritePin(SPI1_CS_GPIO_Port,SPI1_CS_Pin,GPIO_PIN_RESET);
  uint16_t rxdata;
  if (HAL_SPI_TransmitReceive(&hspi1,(uint8_t *)&txdata,(uint8_t *)&rxdata,1,1000) != HAL_OK)
  {
    //未正常执行spi发送接收程序就返回0值
    rxdata = 0;
  }
  HAL_GPIO_WritePin(SPI1_CS_GPIO_Port,SPI1_CS_Pin,GPIO_PIN_SET);
  //正常执行就返回rxdata；
  return rxdata;
}

uint8_t parity_check_uint16(uint16_t data)
{
  int count = 0;

  for (int i = 0; i < 15; i++)
  {
    if (data & (1 << i))
    {
      count++;
    }
  }

  if (count % 2 == 0)
  {
    return 0; //偶校验通过
  }
  else
  {
    return 1; //奇校验通过
  }

}

float Get_Electrical_Angle(void)
{
  float Angle_Temp = 0;
  float Ele_Angele_Temp = 0;
  Angle_Temp = Encoder_Data_Real.Now / 16384.0f * CPR * POLE_PAIR_NUM - Electrical_Angle_Offset;
  Ele_Angele_Temp = fmodf(Angle_Temp,CPR);
  Ele_Angele_Temp = Ele_Angele_Temp >= 0 ? Ele_Angele_Temp : (Ele_Angele_Temp + 2 * PI);
  return Ele_Angele_Temp;
}

uint16_t Encoder_Read(uint16_t cmd)
{
  uint16_t read_data = 0;
  cmd |= 0x4000;
  if(parity_check_uint16(cmd))
  {
    cmd |= 0x8000;
  }
  SPI_ReadWrite_OneByte(cmd);
  while (HAL_SPI_GetState(&hspi1) != HAL_SPI_STATE_READY);
  read_data = SPI_ReadWrite_OneByte(0XC000);
  while (HAL_SPI_GetState(&hspi1) != HAL_SPI_STATE_READY);
  read_data &= 0x3fff;
  return read_data;
}

void Encoder_Init()
{
  Encoder_Data.Now = Encoder_Read(ANGLEUNC);
  Encoder_Data_Real.Now = Encoder_Data.Now;
}